package com.rockwell.robot2015.model.states.running;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.RobotStateMachine;
import com.rockwell.robot2015.model.states.running.sequence.*;

public class RobotRunningStateMachine implements IStateMachine {
	static boolean sensor_L=false;
	static boolean sensor_R=false;
	static boolean sensor_F=false;
	private final static int SMALL_TURN_INDEX = 5;
	class RobotBlocked implements IState {
		
		@Override
		public void enter(IStateMachine machine) {
			machine.robot().sendCommand(RobotAction.Stop, 0);
		}

		@Override
		public void move(IStateMachine machine) {
			IRobotSensors sensor=machine.robot().getSensor(); // this may change based on sensors
			sensor_L=sensor.detectLeftWall();
			sensor_R=sensor.detectRightWall();
			sensor_F=sensor.detectFrontWall();
			if(sensor_L){
				machine.robot().sendCommand(RobotAction.MoveBackward, 20);
				machine.robot().sendCommand(RobotAction.TurnDegree, SMALL_TURN_INDEX);
				machine.robot().sendCommand(RobotAction.MoveForward, 20);
			}
			else if(sensor_R){
				machine.robot().sendCommand(RobotAction.MoveBackward, 20);
				machine.robot().sendCommand(RobotAction.TurnDegree, -SMALL_TURN_INDEX);
				machine.robot().sendCommand(RobotAction.MoveForward, 20);
			}
			else if(sensor_F){
				machine.robot().sendCommand(RobotAction.MoveBackward, 20);
				machine.robot().sendCommand(RobotAction.TurnDegree, -SMALL_TURN_INDEX);
				machine.robot().sendCommand(RobotAction.MoveForward, 20);
			}
			// Detect which corner is blocked and decide whether to 
			// move forward or backward and whether to turn left or right
			
		}

		@Override
		public void exit(IStateMachine machine) {
			onUnblocked();
		}
		
	}
	
	IState m_currentState, m_nextState;

	RobotBlocked m_robotBlocked = new RobotBlocked();
	boolean m_inBlockedState = false;
	
	RobotStateMachine m_mainMachine;
	Robot m_robot;
	
	boolean m_isJogging;
	boolean m_isJogging2;
	boolean m_isJogging3;
	
	FindingTreasureSequence m_findingTreasureSequence;
	ApproachingTreasureSequence m_approachingTreasureSequence;
	PickingTreasureSequence m_pickingTreasureSequence;
	FindingMazeSequence m_findingMazeSequence;
	ApproachingMazeSequence m_approachingMazeSequence;
	FindingMazeEntranceSequence m_findMazeEntranceSequence;
	EnteringMazeSequence m_enteringMazeSequence;
	EnteringMazeDirectSequence m_enteringMazeDirectSequence;
	FindingDropAreaSequence m_findDroppingAreaSequence;
	DroppingTreasureSequence m_droppingTreasureSequence;
	ExitingMazeSequence m_exitingMazeSequence;
	
	public RobotRunningStateMachine(RobotStateMachine mainMachine) {
		m_mainMachine = mainMachine;
		m_robot = mainMachine.robot();
		
		m_findingTreasureSequence = new FindingTreasureSequence(this);
		m_approachingTreasureSequence = new ApproachingTreasureSequence(this);
		m_pickingTreasureSequence = new PickingTreasureSequence(this);
		m_findingMazeSequence = new FindingMazeSequence(this);
		m_approachingMazeSequence = new ApproachingMazeSequence(this);
		m_findMazeEntranceSequence = new FindingMazeEntranceSequence(this);
		m_enteringMazeSequence = new EnteringMazeSequence(this);
		m_enteringMazeDirectSequence = new EnteringMazeDirectSequence(this);
		m_findDroppingAreaSequence = new FindingDropAreaSequence(this);
		m_droppingTreasureSequence = new DroppingTreasureSequence(this);
		m_exitingMazeSequence = new ExitingMazeSequence(this);
		
		m_currentState = RobotRunningState.FindingTreasure;
	}
	
	@Override
	public void changeState(IState state) {
		if (m_currentState != null) {
			m_currentState.exit(this);
		}
		
		m_currentState = state;
		m_currentState.enter(this);
	}

	@Override
	public void reset() {
		m_nextState = RobotRunningState.FindingTreasure;

		m_findingTreasureSequence.reset();
		m_approachingTreasureSequence.reset();
		m_pickingTreasureSequence.reset();
		m_findingMazeSequence.reset();
		m_approachingMazeSequence.reset();
		m_findMazeEntranceSequence.reset();
		m_enteringMazeSequence.reset();
		m_enteringMazeDirectSequence.reset();
		m_findDroppingAreaSequence.reset();
		m_droppingTreasureSequence.reset();
		m_exitingMazeSequence.reset();
	}
	
	@Override
	public void move() {
		if (m_inBlockedState) {
			m_robotBlocked.move(this);
		}
		else if (m_mainMachine.robot().blocked()) {
			m_inBlockedState = true;
			m_robotBlocked.move(this);
		}
		else {
			if (m_nextState != null) {
				changeState(m_nextState);
				m_nextState = null;
			}

			m_currentState.move(this);
		}
	}

	void onUnblocked() {
		m_inBlockedState = false;
	}
	
	@Override
	public IState getState() {
		return m_currentState;
	}

	public void setNewState(RobotRunningState newState) {
		m_nextState = newState;
	}

	@Override
	public Robot robot() {
		return m_mainMachine.robot();
	}

	@Override
	public void waitForResume() {
		m_mainMachine.waitForResume();
	}
	
	/**
	 * Called before entering the Pause state, or before pausing in Step mode.
	 */
	public void beforePause() {
		//m_currentState.exit(this);
		/*
		m_isJogging = m_robot.isJogging();
		m_isJogging2 = m_robot.isJogging2();
		m_isJogging3 = m_robot.isJogging3();
		
		if (m_isJogging || m_isJogging2 || m_isJogging3) {
			m_robot.sendCommand(RobotAction.Stop);
		}
		*/
	}
	
	/**
	 * Called after resumed from the Pause state, or before making a move in Step mode.
	 */
	public void afterResume() {
		//m_currentState.enter(this);
		
		/*
		if (m_isJogging) {
			m_robot.startJogging();
		}
		else if (m_isJogging2) {
			m_robot.startJogging2();
		}
		else if (m_isJogging3) {
			m_robot.startJogging3();
		}
		*/
		m_isJogging = m_isJogging2 = m_isJogging3 = false;
	}

	public FindingTreasureSequence findingTreasureSequence() {
		return m_findingTreasureSequence;
	}
	
	public ApproachingTreasureSequence approachingTreasureSequence() {
		return m_approachingTreasureSequence;
	}
	
	public PickingTreasureSequence pickingTreasureSequence() {
		return m_pickingTreasureSequence;
	}
	
	public FindingMazeSequence findingMazeSequence() {
		return m_findingMazeSequence;
	}
	
	public ApproachingMazeSequence approachingMazeSequence() {
		return m_approachingMazeSequence;
	}
	
	public FindingMazeEntranceSequence findingMazeEntranceSequence() {
		return m_findMazeEntranceSequence;
	}
	
	public EnteringMazeSequence enteringMazeSequence() {
		return m_enteringMazeSequence;
	}
	
	public EnteringMazeDirectSequence enteringMazeDirectSequence() {
		return m_enteringMazeDirectSequence;
	}
	
	public FindingDropAreaSequence findingDropAreaSequence() {
		return m_findDroppingAreaSequence;
	}
	
	public DroppingTreasureSequence droppingTreasureSequence() {
		return m_droppingTreasureSequence;
	}
	
	public ExitingMazeSequence exitingMazeSequence() {
		return m_exitingMazeSequence;
	}
}
